import socket, sys, os, json, struct, config
import threading, time, math, sqlite3, logging
from serial import Serial
from threading import Thread 
import modbus_tk.modbus_tcp as modbus_tcp
from modbus_tk.modbus_rtu import RtuServer
import modbus_tk.defines as cst
import subprocess
from periphery import GPIO

'''
####
Date : 2024-4-29
Notes: VSR实体控制器--server端

Test: 通讯格式
####
'''

robot_logger=logging.getLogger("Controler-Driver")
robot_logger.setLevel(logging.DEBUG)

algorithm_path = "/home/cat/gm/algorithm_run/"

class Controler_Server():
    def __init__(self, host = "0.0.0.0", port = 502):
        super().__init__()
        # 获取本机的IP地址和名字
        host_name = socket.gethostname()
        self.host_add = socket.gethostbyname(host_name)
        self.host = host
        self.port = port

        self.server_tcp_init()

        self.tcpserver = VSR_Server()
        self.algo_th = Thread(target= self.tcpserver.run)
        self.algo_th.start()

        self.robot = robot_control()
        self.master = master_io()

        self.th_update = Thread(target= self.status_update)
        self.th_update.start()

        self.algorithm_th = Thread(target= self.run_algorithm)
        self.algorithm_th.start()

        self.rtu_th = Thread(target= self.run_process)
        self.rtu_th.start()       


    def server_tcp_init(self,):
        try:
            # if self.check_port_conflict():
            #     print(f"Port {self.port} has been used!!!")
            #     return False
            self.server = modbus_tcp.TcpServer(port=self.port,address=self.host,
                                               timeout_in_sec=5)
            self.server.start()
            self.register_init()
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def check_port_conflict(self,):
        try:
            ret = False
            sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            result = sock.connect_ex(self.host,self.port) # 检测有没有服务将这个端口以及IP地址占用
            if result == 0:
                ret = True
            else:
                ret = False
            sock.close()
            return ret
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def register_init(self,):
        self.slave =self.server.add_slave(1)  # 服务器上创建1号从机
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS,0,400) # 创建400个保持寄存器
        self.slave.add_block("fifo",cst.HOLDING_REGISTERS,1024,100) # 创建50个保持寄存器用于存储FIFO
        self.slave.add_block("motor",cst.HOLDING_REGISTERS,10000,50) # 创建50个保持寄存器用于存储电机状态
        self.slave.add_block("di",cst.COILS,1024,50) # 创建50个线圈寄存器用于存储di
        self.slave.add_block("do",cst.COILS,20000,50) # 创建50个线圈寄存器用于存储d0

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    # 485RTU 线程管理
    def run_process(self,):
        rtu_process = None 
        algorithm_name = "rtu_server_online.py"
        file_path = algorithm_path + algorithm_name
        print("modbus_RTU 线程管理启动")
        while True:
            try:
                time.sleep(0.5)
                if self.robot.rut_en==1:
                    time.sleep(0.5)
                    if self.robot.rtu_slave>0:
                        if rtu_process is None:
                            rtu_process = subprocess.Popen(["python3",file_path])
                        elif rtu_process.poll() is not None:  # 程序运行结束
                            rtu_process = subprocess.Popen(["python3",file_path])
                        else:
                            pass
                elif self.robot.rut_en==0:
                    if rtu_process is None:
                        pass
                    elif rtu_process.poll() is None: # 程序在运行
                        rtu_process.kill()
                    else:
                        pass
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def run_algorithm(self,):
        print("视觉算法 线程管理启动")
        while True:
            try:
                time.sleep(0.5)
                num =   self.slave.get_values("robot6",181,1)[0]
                if num !=0:
                    match num:
                        case 1:
                            algorithm_name = "algorithm.py"

                        case 2:
                            algorithm_name = "circle.py"           
                        case 3:
                            algorithm_name = "shape.py"
                        case 4:
                            algorithm_name = "color_lab.py"

                        case 5:
                            algorithm_name = "color_hsv.py"

                        case 6:
                            algorithm_name = "ocr_test.py"           
                        case 7:
                            algorithm_name = "yolov8_detect.py"
                        case 8:
                            algorithm_name = "algorithm8.py"
                        case _:
                            algorithm_name = "algorithm.py"
                    file_path = algorithm_path + algorithm_name

                    self.algo_th  = subprocess.Popen(["python3",file_path])   
                    time.sleep(2)
                    while self.algo_th.poll() is None:
                        time.sleep(0.5)
                    self.slave.set_values("robot6",181,0)
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)

                # 更新主机DI的状态
                m_di = []
                for i in range (0,13):       # 共有13个DI
                    if self.master.get_DI(num=i+1) == True:
                        m_di.append(0)
                    else:
                        m_di.append(1)
                self.slave.set_values("robot6",70,m_di)   # 获取DI并且刷新到MODBUS表中
                time.sleep(0.005)

                # 更新机器人的状态
                status = []
                slider_status = []
                s = self.robot.status_global
                if s != None:
                    match s["state"]:
                        case "Idle":
                            status = status + [1]
                        case "Alarm":
                            status = status + [2]
                        case "Home":
                            status = status + [3]
                        case "Run":
                            status = status + [4]
                        case _:
                            status = status + [0]
                    ang = s["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])

                    ang = s["coordinate"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])
                        if i ==6:
                            slider_status.append(a[0])
                            slider_status.append(a[1])
                    
                    status.append(self.robot.robot_type_number)     # 增加机器人的类型
                    status.append(self.tcpserver.connect_flag)  # 增加数字仿真服务状态
                    status.append(self.robot.rtu_slave)

                    self.slave.set_values("robot6",0,status)
                    self.slave.set_values("motor",10000,slider_status)# 这里是使用modbus进行通讯IOA 


            except Exception as e:
                robot_logger.error(e,exc_info=True)

    # 启动在线线程
    def online(self):
        file_path = '/home/cat/gm/controler/process_dis.py'
        try:

            if os.path.exists(file_path):
                self.online_p = subprocess.Popen(f'python3 {file_path}',shell=True,preexec_fn=os.setsid)
            else:
                raise Exception(f"No {file_path}")
        except Exception as e:
            print(e)
        # 启动在线线程

    def dorun(self,):
        self.online()
        Mdo_flag = [0,0,0,0,0]
        bo = 0x00
        while True:
            try:
                time.sleep(0.1)

                # 1轮询执行DI的状态
                Mdo_list = self.slave.get_values("robot6",86,8)

                for i in range(5):   # 两个电机口和三个LED灯
                    if Mdo_list[i] == Mdo_flag[i]:
                        pass
                    else:
                        Mdo_flag[i]= Mdo_list[i]
                        match Mdo_list[i]:
                            case 0:
                                self.master.set_LED(num=i,value=False)  # DO输出低电平
                            case 1:
                                self.master.set_LED(num=i,value=True)   # DO输出高电平


                # 2执行DO的值
                if Mdo_list[7] == 1:
                    self.master.set_DO(num=Mdo_list[5],value=Mdo_list[6])
                    self.slave.set_values("do",20000+Mdo_list[5],Mdo_list[6])  # 向IOA寄存器中写值
                    self.slave.set_values("robot6",93,0)   # 将MODBUS表中的使能清除掉

                config.set_value("BO0",self.master.bo_out)  



                # 3执行机器人的指令
                cmd = self.slave.get_values("robot6",49,2)
                if cmd[0] == 1:
                    P = []
                    motion = self.slave.get_values("robot6",31,18)
                    for i in range(0,14,2):
                        P.append(self.int2float(motion[i],motion[i+1]))
                    self.robot.move(P=P,speed=motion[17],coordinate=motion[14],is_relative=motion[15],mode=motion[16])
                    self.slave.set_values("robot6",49,0)
                
                # 复位等命令
                match cmd[1]:
                    case 0:
                        pass
                    case 1:
                        self.robot.home_1axis(1)
                        self.slave.set_values("robot6",50,0)
                        
                    case 2:
                        self.robot.home_1axis(2)
                        self.slave.set_values("robot6",50,0)
                        
                    case 3:
                        self.robot.home_1axis(3)
                        self.slave.set_values("robot6",50,0)
                        
                    case 4:
                        self.robot.home_1axis(4)
                        self.slave.set_values("robot6",50,0)
                    case 5:
                        self.robot.home_1axis(5)
                        self.slave.set_values("robot6",50,0)

                    case 6:
                        self.robot.home_1axis(6)
                        self.slave.set_values("robot6",50,0)

                    case 7:
                        self.robot.home_1axis(7)
                        self.slave.set_values("robot6",50,0)

                    case 8:
                        self.robot.home()
                        self.slave.set_values("robot6",50,0)

                    case 9:
                        self.robot.home_6axis_in_turn()
                        self.slave.set_values("robot6",50,0)

                    case 10:
                        self.robot.home_7axis()
                        self.slave.set_values("robot6",50,0)

                    case 11:
                        self.robot.unlock_all_axis()
                        self.slave.set_values("robot6",50,0)

                    case 12:
                        self.robot.stop()
                        self.slave.set_values("robot6",50,0)

                    case 13:
                        self.robot.go_to_zero()
                        self.slave.set_values("robot6",50,0)


                #4 执行485模式


            except Exception as e:
                robot_logger.error(e,exc_info=True)


class master_io():
    def __init__(self):
        self.gobal_do = 0

        self.A0 = 0
        self.A1 = 1
        self.A2 = 2
        self.A3 = 3
        self.A4 = 4
        self.A5 = 5
        self.A6 = 6
        self.A7 = 7

        self.B0 = 8
        self.B1 = 9
        self.B2 = 10
        self.B3 = 11
        self.B4 = 12
        self.B5 = 13
        self.B6 = 14
        self.B7 = 15      

        self.C0 = 16
        self.C1 = 17 
        self.C2 = 18
        self.C3 = 19 
        self.C4 = 20
        self.C5 = 21
        self.C6 = 22
        self.C7 = 23

        self.chip0 = "/dev/gpiochip0"  # chip0
        self.chip1 = "/dev/gpiochip1"  # chip1
        self.chip2 = "/dev/gpiochip2"  # chip2
        self.chip3 = "/dev/gpiochip3"  # chip3
        self.chip4 = "/dev/gpiochip4"  # chip4

        self.BUTTON1 = GPIO(self.chip1, self.A0, "in")  # 拨码按钮   
        self.BUTTON2 = GPIO(self.chip1, self.A1, "in")  # 离线按钮
        self.BUTTON3 = GPIO(self.chip1, self.A4, "in")  # 复位按钮

        self.DI_9 = GPIO(self.chip1, self.B2, "in")  # DI9 端口
        self.DI_8 = GPIO(self.chip1, self.B1, "in")  # DI8 端口

        self.DI_7 = GPIO(self.chip4, self.C3, "in")  # DI7 端口
        self.DI_6 = GPIO(self.chip4, self.C5, "in")  # DI6 端口
        self.DI_5 = GPIO(self.chip4, self.C2, "in")  # DI5 端口
        self.DI_4 = GPIO(self.chip3, self.B4, "in")  # DI4 端口
        self.DI_3 = GPIO(self.chip3, self.C1, "in")  # DI3 端口
        self.DI_2 = GPIO(self.chip3, self.B6, "in")  # DI2 端口
        self.DI_1 = GPIO(self.chip3, self.B5, "in")  # DI1 端口
        self.DI_0 = GPIO(self.chip1, self.B0, "in")  # DI0 端口

        self.Motor_0 = GPIO(self.chip3, self.B2, "out")  # 电机正转
        self.Motor_1 = GPIO(self.chip3, self.B3, "out")  # 电机反转
        self.LED2 = GPIO(self.chip3, self.A6, "out")  # 绿色
        self.LED3 = GPIO(self.chip3, self.A7, "out")  # 蓝色
        # self.LED4 = GPIO(self.chip4, self.C2, "out")  # 红色

        self.DO0 = GPIO(self.chip3, self.B1, "out")  # DO输出1
        self.DO1 = GPIO(self.chip3, self.B0, "out")  # DO输出2
        self.DO2 = GPIO(self.chip4, self.C6, "out")  # DO输出3
        self.DO3 = GPIO(self.chip4, self.C4, "out")  # DO输出4

        self.bo_out =0x00  

    def bo_count(self,BO,bo_num,value):# 定义一个按位计算的函数
        # BO 是一个8位的16进制数
        # bo_bit 是第几个IO
        # value 是DO的值
        # 将DO状态上传IOA服务器

        try:
            bit_num = 0b00000000
            if value ==0:
                match bo_num:
                    case 0 :
                        bit_num = 0b11111110
                    case 1 :
                        bit_num = 0b11111101

                    case 2 :
                        bit_num = 0b11111011
                    case 3 :
                        bit_num = 0b11110111
                    case 4 :
                        bit_num = 0b11101111
                    case 5 :
                        bit_num = 0b11011111
                    case 6 :
                        bit_num = 0b10111111
                    case 7 :
                        bit_num = 0b01111111
                BO  =  BO & bit_num
            
            elif value ==1:
                match bo_num:
                    case 0:
                        bit_num = 0b00000001
                    case 1:
                        bit_num = 0b00000010
                    case 2:
                        bit_num = 0b00000100
                    case 3:
                        bit_num = 0b00001000
                    case 4:
                        bit_num = 0b00010000
                    case 5:
                        bit_num = 0b00100000
                    case 6:
                        bit_num = 0b01000000
                    case 7:
                        bit_num = 0b10000000
                BO  =  BO | bit_num
            return BO            

        except Exception as e:
            print(e)

    def get_DI(self,num :int =1):
        try:
            value = -1

            match num:
                case 1:
                    value = self.BUTTON1.read()
                case 2:
                    value = self.BUTTON2.read()   
                case 3:
                    value = self.BUTTON3.read() 
                case 4:
                    value = self.DI_0.read() 
                case 5:
                    value = self.DI_1.read() 
                case 6:
                    value = self.DI_2.read() 
                case 7:
                    value = self.DI_3.read() 
                case 8:
                    value = self.DI_4.read() 
                case 9:
                    value = self.DI_5.read() 
                case 10:
                    value = self.DI_6.read() 
                case 11:
                    value = self.DI_7.read() 
                case 12:
                    value = self.DI_8.read() 
                case 13:
                    value = self.DI_9.read() 
              
            return value
        except Exception as e:
            robot_logger.error(e,exc_info=True) 

    # 这里是设置的四个灯的状态资源
    def set_LED(self,num,value=True):
        try:
            match num:
                case 0:
                    self.Motor_0.write(value)
                    if value:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=1)
                    else:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=0)
                case 1:
                    self.Motor_1.write(value)
                case 2:
                    self.LED2.write(value)
                case 3:
                    self.LED3.write(value)
                case 4:
                    pass
                    # self.LED4.write(value)


        except Exception as e:
            robot_logger.error(e,exc_info=True)

    #设置DO只需要调用这个函数
    def set_DO(self,num,value=1):
        try:
            value_bool = True
            if value == 0:
                value_bool =False
            elif value == 1:
                value_bool = True
            match num:
                case 0:
                    self.DO0.write(value_bool)
                case 1:
                    self.DO1.write(value_bool)
                case 2:
                    self.DO2.write(value_bool)
                case 3:
                    self.DO3.write(value_bool)

            self.bo_out = self.bo_count(self.bo_out,num,value=value)
        except Exception as e:
            robot_logger.error(e,exc_info=True)

class VSR_Server:
    def __init__(self):
        self.connect_flag = 0
        self.tcpServer()
        self.robot = robot_control()

        self.robot_type = 0
        self.global_state = [1,1,1]
        ### IO 状态
        self.IO = [0,1,1,1,1,1,1,1]
        self.default_speed = 1000
        self.lock_object = threading.RLock()
    
    def tcpServer(self):
        self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)
        self.tcp_socket.bind(('0.0.0.0', 8090))
        self.tcp_socket.listen(10)
    
    def run(self,):
        while True:
            self.connect_flag = 0            
            print('waiting for connection...')
            self.client_socket, self.client_address = self.tcp_socket.accept()
            print('connected from:', self.client_address)
            time.sleep(0.01)
            
            while True:
                try:

                    self.send_data_process()
                    time.sleep(0.05)

                    # recv_data = self.client_socket.recv(1024)
                    # if len(recv_data) > 10:
                    #     self.recv_data_process(recv_data)
                    # else:
                    #     self.send_data_process()
                    #     time.sleep(0.05)
                
                except Exception as e:
                    robot_logger.error(e,exc_info=True)
                    self.client_socket.close()
                    break

            # self.client_socket.close()
            self.tcp_socket.listen(10)              

                # ## 遍历每个JSON字符串并尝试解析  
                # for json_str in json_strings:
                #     ## 跳过空字符串  
                #     if not json_str:  
                #         continue  
                #     try:  
                #         ## 解析JSON字符串为Python字典  
                #         data = json.loads(json_str)
                #         if data['COM1'] == 'M-Mode':
                #             self.recv_data_process(recv_data)
                #         elif data['COM1'] == 'A-Mode':
                #             self.send_data_process()
                #             time.sleep(0.05)
                            
                #     except json.JSONDecodeError as e:  
                #         print(f"JSON解析错误: {e}")      

                    
    def recv_data_process(self, data):
        time.sleep(0.01)
        ### 示教控制 manaual
        self.parse_data(data)

    def parse_data(self, recv_data):
        ### 按换行符分割数据字符串为多个JSON字符串  
        json_strings = recv_data.strip().split('\n')  
        
        ## 遍历每个JSON字符串并尝试解析  
        for json_str in json_strings:
            ## 跳过空字符串  
            if not json_str:  
                continue  
            try:  
                ## 解析JSON字符串为Python字典  
                data = json.loads(json_str)  
                
                ## 检查是否是包含"type"和"C0M1"的JSON对象  
                if 'type' in data and data['type'] == 'M21' or data['type'] == 'M20' and 'C0M1' in data:  
                    command = data['C0M1']
                    command_2 = data['COM2']

                    if data['type'] == 'M21':
                        if "up" in command:
                            gcode = f"!\n"
                            self.robot.serial_com1.write(gcode.encode())
                            time.sleep(0.5)
                        elif "down" in command : 
                            self.lock_object.acquire()
                            s_data = self.robot.status_global
                            self.lock_object.acquire()
                            time.sleep(0.01)
                            angle_current = s_data["deviceData"]["angle"]
                            angle_current = [int(i) for i in angle_current]          
                            ## 根据不同的命令执行不同的操作
                            match command:
                                case "1_+_down" :
                                    gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "1_-_down" :
                                    gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_+_down" :
                                    gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_-_down" :
                                    gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_+_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_-_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_+_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_-_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "5_+_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "5_-_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-199 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "6_+_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "6_-_down" :
                                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "restart":
                                    gcode = f"$h0\n"
                                    print("复位操作")
                                case "stop":
                                    gcode = f"%\n"
                                    print("急停操作")
                            
                            self.robot.serial_com1.write(gcode.encode())
                        else:
                            angle = json.loads(command)
                            self.robot.move(P = angle, coordinate=0)                 
                        

                        if "up" in command_2:
                            gcode_2 = f"!\n"
                            self.robot.serial_com2.write(gcode_2.encode())
                            time.sleep(0.5)
                        
                        elif "down" in command_2 : 
                            s_data = self.robot.status_global_
                            angle_current_ = s_data["deviceData"]["angle"]
                            angle_current = [int(i) for i in angle_current_]          
                            ## 根据不同的命令执行不同的操作
                            match command_2:
                                case "1_+_down" :
                                    gcode_2 = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "1_-_down" :
                                    gcode_2 = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_+_down" :
                                    gcode_2 = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_-_down" :
                                    gcode_2 = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_+_down" :
                                    gcode_2 = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_-_down" :
                                    gcode_2 = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_+_down" :
                                    gcode_2 = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_-_down" :
                                    gcode_2 = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "restart":
                                    gcode_2 = f"$h0\n"
                                    print("复位操作")
                                case "stop":
                                    gcode_2 = f"%\n"
                                    print("急停操作")
                            
                            self.robot.serial_com2.write(gcode_2.encode())
                        else:
                            angle = json.loads(command_2)
                            self.robot.move(P = angle, coordinate=0)
                    
                    if data['type'] == 'M20':
                        if "up" in command:
                            gcode = f"!\n"
                            self.robot.serial_com1.write(gcode.encode())
                            time.sleep(0.5)
                        elif "down" in command : 
                            self.lock_object.acquire()
                            s_data = self.robot.status_global
                            self.lock_object.acquire()
                            time.sleep(0.01)
                            angle_current = s_data["deviceData"]["angle"]
                            angle_current = [int(i) for i in angle_current]          
                            ## 根据不同的命令执行不同的操作
                            match command:
                                case "1_+_down" :
                                    gcode = "M20 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "1_-_down" :
                                    gcode = "M20 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_+_down" :
                                    gcode = "M20 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_-_down" :
                                    gcode = "M20 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_+_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_-_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_+_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_-_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "5_+_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "5_-_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z{} A{} B-199 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "6_+_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "6_-_down" :
                                    gcode = "M20 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "restart":
                                    gcode = f"$h0\n"
                                    print("复位操作")
                                case "stop":
                                    gcode = f"%\n"
                                    print("急停操作")
                            
                            self.robot.serial_com1.write(gcode.encode())
                        else:
                            angle = json.loads(command)
                            self.robot.move(P = angle, coordinate=0)                 
                        

                        if "up" in command_2:
                            gcode_2 = f"!\n"
                            self.robot.serial_com2.write(gcode_2.encode())
                            time.sleep(0.5)
                        
                        elif "down" in command_2 : 
                            s_data = self.robot.status_global_
                            angle_current_ = s_data["deviceData"]["angle"]
                            angle_current = [int(i) for i in angle_current_]          
                            ## 根据不同的命令执行不同的操作
                            match command_2:
                                case "1_+_down" :
                                    gcode_2 = "M20 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "1_-_down" :
                                    gcode_2 = "M20 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_+_down" :
                                    gcode_2 = "M20 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "2_-_down" :
                                    gcode_2 = "M20 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_+_down" :
                                    gcode_2 = "M20 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "3_-_down" :
                                    gcode_2 = "M20 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_+_down" :
                                    gcode_2 = "M20 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "4_-_down" :
                                    gcode_2 = "M20 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                            angle_current[3],angle_current[4],angle_current[5])
                                case "restart":
                                    gcode_2 = f"$h0\n"
                                    print("复位操作")
                                case "stop":
                                    gcode_2 = f"%\n"
                                    print("急停操作")
                            
                            self.robot.serial_com2.write(gcode_2.encode())
                        else:
                            angle = json.loads(command_2)
                            self.robot.move(P = angle, coordinate=1) 

            except json.JSONDecodeError as e:  
                print(f"JSON解析错误: {e}")

    # 定义通讯协议格式  
    def build_packet(self,data_size, packet_type, message_body):  
        # 构建报头  
        header = json.dumps({  
            "data_size": data_size,  
            "type": packet_type  
        }).encode('utf-8')
        
        # 计算报头长度（4位bit，即半个字节，需要转换为字节长度）  
        header_length = len(header)
        
        # 构建帧头
        # frame_header = struct.pack('>H', header_length)
        frame_header = header_length.to_bytes(4, 'little')
        
        # 完整的数据包  
        packet = frame_header + header + message_body.encode('utf-8')
        return packet 

    # 定义消息体  
    def create_message_body(self,robot_type, state, com1, com2, rs485, com1Float, com2Float,io):  
        message_body = {  
            "robot_type": robot_type,  
            "state": state,  
            "com1": com1,  
            "com2": com2,  
            "RS485": rs485, 
            "com1Float": com1Float,
            "com2Float": com2Float,
            "IO": io  
        }  
        return json.dumps(message_body)

    def robot_type_send(self):
        a = self.robot.robot_type_number 
        b = self.robot.robot_type_number_2
        c = "{}_{}".format(a, b)
        return c

    def send_data_process(self, ):
        P = list()
        F = list()
        P.append(config.get_value("j0"))  
        P.append(config.get_value("j1"))
        P.append(config.get_value("j2"))
        P.append(config.get_value("j3"))
        P.append(config.get_value("j4"))
        P.append(config.get_value("j5"))
        
        F.append(config.get_value("j7"))
        F.append(config.get_value("j8"))
        F.append(config.get_value("j9"))
        F.append(config.get_value("j10"))
        
        if P is not None:
            P = P
        else:
            P = [0,0,0,0,0,0]
        
        if F is not None:
            F = F
        else:
            F = [0,0,0,0]
        

        robot_type = self.robot_type_send()
        state = self.global_state
        
        com1 = P
        com2 = F
        rs485 = ""
        com1Float = config.get_value("FO[1]")
        com2Float = config.get_value("FO[1]")
        io = self.IO
        
        ### 消息体
        message_body = self.create_message_body(robot_type, state, com1, com2, rs485, com1Float, com2Float, io)
        ### 消息体长度
        data_size = len(message_body)
        ### 控制器类型：实体控制器
        packet_type = "Physical" 
        ### 构建数据包 
        packet = self.build_packet(data_size, packet_type, message_body)
        self.client_socket.sendall(packet)


####
#com1 六轴机器人#com2 四轴机器人 
####
class robot_control:
    def __init__(self,):
        self.serial_com1 = Serial('/dev/ttyS7', 115200, timeout=0.5)
        self.thread_update_ = Thread(target=self.task_com1,)
        self.thread_update_.start()
        self.status_global = None

        self.serial_com2 = Serial('/dev/ttyUSB0', 115200, timeout=0.5)
        self.thread_update = Thread(target=self.task_com2,)
        self.thread_update.start()
        self.status_global_ = None


        time.sleep(0.01)
        self.default_speed = 2000
        # 这里来表示机器人类型。0：代表无机器人，1:代表是六轴，2：代表是四轴
        self.robot_type_number = 0 
        self.robot_type_number_2 = 0

        # 定义RTU服务内容
        self.rtu_slave = 0
        self.rut_en = 0

        ########################数据库存储状态############################
        filename = 'ktr6.db'
        file_dir = "/home/cat/gm/data/"
        # # 使用系统分隔符连接目录和文件
        # file_path = os.sep.join([file_dir, filename])
        # 使用路径模块连接
        file_path = os.path.join(file_dir, filename)
        self.conn = sqlite3.connect(file_path,timeout=10,check_same_thread=False)

    def send_msg(self,message, wait_ok=False, wait_idle=True , terminator=os.linesep, com_flag = None):
        try:
            # 自动添加换行符
            if not message.endswith(terminator):
                message += terminator
            # 串口发送数据，编码为utf-8
            if com_flag == 1:
                self.serial_com1.write(message.encode('utf-8'))
            elif com_flag == 2:
                self.serial_com2.write(message.encode('utf-8'))
            
            return True
        except Exception as e:
            # 日志写入串口设备写入异常
            
            robot_logger.error(e,exc_info=True)
            return False

    def robot_type_update(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            # gcode = f'$h\r\n'
            time.sleep(0.1) 
            
            if com_flag == 1:
                self.serial_com1.write(gcode.encode('utf-8'))
                # self.send_msg(gcode,com_flag=1)
                time.sleep(1)
                data = self.serial_com1.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com1:" + recive)
            time.sleep(0.01)
            
            if "Ry" in recive:
            # if "Mirobot" in recive:
                self.robot_type_number = 1
            elif "E4" in recive:
                self.robot_type_number = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robot_type_update_2(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            # gcode = f'$h\r\n'
            time.sleep(0.1) 
            
            if com_flag == 2:
                self.serial_com2.write(gcode.encode('utf-8'))
                # self.send_msg(gcode,com_flag=2)
                time.sleep(1)
                data = self.serial_com2.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com2:" + recive)
            
            if "Mirobot" in recive:
                self.robot_type_number_2 = 1
            elif "E4" in recive:
                self.robot_type_number_2 = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json(self,data,):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            
            if status_date !=None:
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number

                    }
            
            if status != None:

                self.status_global = status
                self.send_tcp_data(status)

            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json_2(self,data,):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            
            if status_date !=None:
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number_2

                    }
            
            if status != None:
                    self.status_global_ = status
                    self.send_tcp_data_2(status)
            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)
  

    ### 将浮点数打包成32位2进制数据
    def float2bytes(self,f):
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]  

    #### 数据---TCP
    def send_tcp_data(self,data):
        try:
            j0  = data["angle"][0]
            j1  = data["angle"][1]
            j2  = data["angle"][2]
            j3  = data["angle"][3]
            j4  = data["angle"][4]
            j5  = data["angle"][5]
            j6  = data["coordinate"][6]


            # robotdata_byte0=(self.float2bytes(j0))
            # robotdata_byte1=(self.float2bytes(j1))
            # robotdata_byte2=(self.float2bytes(j2))
            # robotdata_byte3=(self.float2bytes(j3))
            # robotdata_byte4=(self.float2bytes(j4))
            # robotdata_byte5=(self.float2bytes(j5))
            # robotdata_byte6=(self.float2bytes(j6))
            
            ###将MS数据存入字典
            config.set_value("j0",j0) 
            config.set_value("j1",j1)
            config.set_value("j2",j2)
            config.set_value("j3",j3)
            config.set_value("j4",j4)
            config.set_value("j5",j5)
            config.set_value("FO[1]",value=j6)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def send_tcp_data_2(self,data):
        try:
            j7  = data["angle"][0]
            j8  = data["angle"][1]
            j9  = data["angle"][2]
            j10  = data["angle"][3]

            # robotdata_byte7=(self.float2bytes(j7))
            # robotdata_byte8=(self.float2bytes(j8))
            # robotdata_byte9=(self.float2bytes(j9))
            # robotdata_byte10=(self.float2bytes(j10))

            config.set_value("j7",j7) 
            config.set_value("j8",j8)
            config.set_value("j9",j9)
            config.set_value("j10",j10)

        except Exception as e:
            robot_logger.error(e,exc_info=True)


    def set_robot_state(self,data):
        try :
            c = self.conn.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state where ID=1")
            data_en = en.fetchall()

            if data_en is not None:
                # 检测是否有G代码要执行
                if data_en[0][0] == 1:
                    gcode = c.execute("SELECT GCODE from Robot_state where ID=1")
                    data_gcode = gcode.fetchall()
                    self.send_msg(data_gcode[0][0])
                    c.execute("UPDATE Robot_state set EN = 0 where ID=1")
                    # self.conn.commit() 
                self.rtu_slave = data_en[0][1]
                self.rut_en = data_en[0][2]  # 更新服务状态
                
                # 将机器人状态更新到数据库里。
                if state=="Idle":
                    c.execute("UPDATE Robot_state set STATE = 'Idle' where ID=1")
                elif state=="Alarm":

                    c.execute("UPDATE Robot_state set STATE = 'Alarm' where ID=1")
                elif state=="Home":

                    c.execute("UPDATE Robot_state set STATE = 'Home' where ID=1")
                elif state=="Run":

                    c.execute("UPDATE Robot_state set STATE = 'Run' where ID=1")

                self.conn.commit()
            else:
                print("No record found with ID=1 in Robot_state table.")
        except Exception as e:
            print(e)

    def set_robot_state_2(self,data):
        try :
            c = self.conn.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state where ID=2")
            data_en = en.fetchall()

            if data_en is not None:
                # 检测是否有G代码要执行
                if data_en[0][0] == 1:
                    gcode = c.execute("SELECT GCODE from Robot_state where ID=2")
                    data_gcode = gcode.fetchall()
                    self.send_msg(data_gcode[0][0])
                    c.execute("UPDATE Robot_state set EN = 0 where ID=2")
                    # self.conn.commit() 
                self.rtu_slave = data_en[0][1]
                self.rut_en = data_en[0][2]  # 更新服务状态
                
                # 将机器人状态更新到数据库里。
                if state=="Idle":
                    c.execute("UPDATE Robot_state set STATE = 'Idle' where ID=2")
                elif state=="Alarm":

                    c.execute("UPDATE Robot_state set STATE = 'Alarm' where ID=2")
                elif state=="Home":

                    c.execute("UPDATE Robot_state set STATE = 'Home' where ID=2")
                elif state=="Run":

                    c.execute("UPDATE Robot_state set STATE = 'Run' where ID=2")

                self.conn.commit()
            else:
                print("No record found with ID=2 in Robot_state table.")
        except Exception as e:
            print(e)
   

    #### 六轴
    def task_com1(self,):
        type_flag_count = 0   
        while True:
            try:
                gcode = f"?"
                self.send_msg(gcode, com_flag=1)
                time.sleep(1)
                ### 从串口读取数据 
                data = self.serial_com1.read_all()
                print(f"task_com1  Gcode ? recv data is {data}")
                ### 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    ### 进来第一次询问
                    if self.robot_type_number == 0:  
                        self.robot_type_update(com_flag=1)

                else:
                    type_flag_count +=1

                    # print(type_flag_count)

                    if type_flag_count == 10:
                        self.robot_type_number = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    self.Kata_status2json(data)
                    self.set_robot_state(self.status_global["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)
    #### 四轴
    def task_com2(self,):
        type_flag_count = 0
        while True:
            try:
                gcode = f"?"
                self.send_msg(gcode, com_flag=2)
                time.sleep(1)
                ### 从串口读取数据 
                data = self.serial_com2.read_all()
                print(f"task_com2  Gcode ? recv data is {data}")

                ### 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    ### 进来第一次询问
                    if self.robot_type_number_2 == 0:  
                        self.robot_type_update_2(com_flag=2)

                else:
                    type_flag_count +=1

                    # print(type_flag_count)

                    if type_flag_count == 10:
                        self.robot_type_number_2 = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    self.Kata_status2json_2(data)
                    time.sleep(0.1)
                    self.set_robot_state_2(self.status_global_["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)

#### 机器人Gcode运动控制指令
####
    def home(self, has_slider=False):
        '''机械臂Homing'''
        if has_slider:
            return self.home_7axis()
        else:
            print("home")
            return self.home_6axis()

    def home_1axis(self, axis_id):
        '''单轴Homing'''
        if not isinstance(axis_id, int) or not (axis_id >= 1 and axis_id <= 7):
            return False
        msg = f'$h{axis_id}'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis(self):
        '''六轴Homing'''
        msg = f'$h'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis_in_turn(self):
        '''六轴Homing, 各关节依次Homing'''
        msg = f'$hh'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
    
    def stop(self):
        '''6轴暂停'''
        # msg = f'!'
        # self.send_msg(msg, wait_ok=False, wait_idle=False)
        # time.sleep(0.5)
        msg = f'%'
        return self.send_msg(msg, wait_ok=False, wait_idle=False)

    def home_7axis(self):
        '''七轴Homing(本体 + 滑台)'''
        msg = f'$h0'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
        
    def unlock_all_axis(self):
        '''解锁各轴锁定状态'''
        msg = 'M50'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)
        
    def go_to_zero(self):
        '''回零-运动到名义上的各轴零点'''
        msg = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def set_speed(self, speed):
        '''设置转速'''
        # 转换为整数
        speed = int(speed)
        # 检查数值范围是否合法
        if speed <= 0 or speed > 6000:
            robot_logger.error(f"Illegal movement speed {speed}")
            return False
        # 发送指令
        msg = f'F{speed}'
        return self.send_msg(msg, wait_ok=None, wait_idle=None)

    def set_hard_limit(self, enable):
        '''
        开启硬件限位
        '''
        msg = f'$21={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def set_soft_limit(self, enable):
        '''开启软限位
        注: 请谨慎使用
        '''
        msg = f'$20={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def format_float_value(self, value):
        if value is None:
            return value
        if isinstance(value, float):
            # 精确到小数点后两位数
            return round(value , 2)
        else:
            return value
    
    def generate_args_string(self, instruction, pairings):
        '''生成参数字符'''
        args = [f'{arg_key}{self.format_float_value(value)}' for arg_key, value in pairings.items() if value is not None]

        return ' '.join([instruction] + args)
    
    def move(self, P, speed = None ,coordinate= 1, is_relative=0,mode =0, wait_ok=None):

        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        instruction = 'M20 G90 G1'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        m1 = "M20"             # 世界坐标系
        pairings = {'X': P[0], 'Y': P[1], 'Z': P[2], 'A': P[3], 'B': P[4], 'C': P[5], 'D':P[6], 'F': speed}
        if coordinate ==0:
            m1 = "M21"      # 轴角坐标系
        m2 = "G90"
        if is_relative:
            m2 = 'G91'       
        m3 ="G00"
        if mode == 1:
            m3 = "G01"

        instruction = f"{m1} {m2} {m3} "
        
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def circular_interpolation(self, ex, ey, radius, is_cw=True, speed=None, wait_ok=None):
        '''圆弧插补
        在XY平面上, 从当前点运动到相对坐标(ex, ey).半径为radius
        `is_cw`决定圆弧是顺时针还是逆时针.
        '''
        # 判断是否合法
        distance = math.sqrt(ex**2 + ey**2)
        if distance > (radius * 2):
            self.logger.error(f'circular interpolation error, target posi is too far')
            return False

        instruction = None
        if is_cw:
            instruction = 'M20 G91 G02'
        else:
            instruction = 'M20 G91 G03'
        
        pairings = {'X': ex, 'Y': ey, 'R': radius, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def set_door_lift_distance(self, lift_distance):
        '''设置门式轨迹规划抬起的高度'''
        msg = f"$49={lift_distance}"
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def door_interpolation(self, x=None, y=None, z=None, a=None, b=None, c=None, speed=None, is_relative=False, wait_ok=None):
        '''门式插补'''
        instruction = 'M20 G90 G05'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        if is_relative:
            instruction = 'M20 G91 G05'
        
        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)



class TPU():
    def __init__(self,):
        self.serial = None
        self.logger = logging.getLogger('TPU')
        self.robot_control = robot_control()
        self.comm = VSR_Server()
        self.lock_object = threading.RLock()

    def run(self,):
        '''接收、解析json数据'''
        self.client_socket, self.client_address = self.comm.tcp_socket.accept()
        data = self.client_socket.accept()    
        recv_data = json.loads(data)
        self.parse_data(recv_data)
        time.sleep(0.1)
        self.client_socket.close()

    def parse_data(self, recv_data):
        ### 按换行符分割数据字符串为多个JSON字符串  
        json_strings = recv_data.strip().split('\n')  
        
        ## 遍历每个JSON字符串并尝试解析  
        for json_str in json_strings:
            ## 跳过空字符串  
            if not json_str:  
                continue  
            try:  
                ## 解析JSON字符串为Python字典  
                data = json.loads(json_str)  
                
                ## 检查是否是包含"type"和"C0M1"的JSON对象  
                if 'type' in data and data['type'] == 'M21' or data['type'] == 'M20' and 'C0M1' in data:  
                    command = data['C0M1']
                    command_2 = data['COM2']

                    if "up" in command:
                        gcode = f"!\n"
                        self.robot_control.serial_com1.write(gcode.encode())
                        time.sleep(0.5)
                    elif "down" in command : 
                        self.lock_object.acquire()
                        s_data = self.robot_control.status_global
                        self.lock_object.acquire()
                        time.sleep(0.01)
                        angle_current = s_data["deviceData"]["angle"]
                        angle_current = [int(i) for i in angle_current]          
                        ## 根据不同的命令执行不同的操作
                        match command:
                            case "1_+_down" :
                                gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "1_-_down" :
                                gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "2_+_down" :
                                gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "2_-_down" :
                                gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "3_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "3_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "4_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "4_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "5_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "5_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-199 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "6_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "6_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "restart":
                                gcode = f"$h0\n"
                                print("复位操作")
                            case "stop":
                                gcode = f"%\n"
                                print("急停操作")
                        
                        self.robot_control.serial_com1.write(gcode.encode())
                    else:
                        angle = json.loads(command)
                        self.robot_control.move(P = angle, coordinate=0)                 
                    

                    if "up" in command_2:
                        gcode_2 = f"!\n"
                        self.robot_control.serial_com2.write(gcode_2.encode())
                        time.sleep(0.5)
                    
                    elif "down" in command_2 : 
                        s_data = self.robot_control.status_global_
                        angle_current_ = s_data["deviceData"]["angle"]
                        angle_current = [int(i) for i in angle_current_]          
                        ## 根据不同的命令执行不同的操作
                        match command_2:
                            case "1_+_down" :
                                gcode_2 = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "1_-_down" :
                                gcode_2 = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "2_+_down" :
                                gcode_2 = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "2_-_down" :
                                gcode_2 = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "3_+_down" :
                                gcode_2 = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "3_-_down" :
                                gcode_2 = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "4_+_down" :
                                gcode_2 = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "4_-_down" :
                                gcode_2 = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5])
                            case "restart":
                                gcode_2 = f"$h0\n"
                                print("复位操作")
                            case "stop":
                                gcode_2 = f"%\n"
                                print("急停操作")
                        
                        self.robot_control.serial_com2.write(gcode_2.encode())
                    else:
                        angle = json.loads(command_2)
                        self.robot_control.move(P = angle, coordinate=0) 

            except json.JSONDecodeError as e:  
                print(f"JSON解析错误: {e}")

server = Controler_Server()
server.dorun()